// ESP32基础库
#include "Arduino.h"
// 自定义头文件
#include "headfile.h"

volatile uint8_t PID_Flag = 0;
void TIM_Interrupt_0(void)
{
    PID_Flag = 1;
}

// 陀螺仪的数据有时候会漂，怀疑是初始化没有初始好
void setup() {

    // ======================================初始化======================================
#if SERIAL_BT
    Serial_BT_Init("ESP32_IOT");       // 蓝牙比较占用初始化时间,调试的时候使用,比赛的时候注释掉
#endif

    Serial_Init(Serial,115200);        // 串口初始化

    MPU6050_AND_DMP_Init();            // MPU6050 + DMP初始化

    Encoder_Init();                    // 编码器初始化

    KEY_Init();                        // 按键初始化 + Timer3(10ms)用于按键扫描

    Motor_Init();                      // 电机初始化

    TIM_Init(Timer0,10);               // 用于PID计算
    TIM_Init(Timer1,10);               // 用于获取编码器计数值
    // ======================================初始化======================================

    // 创建双核任务
    xTaskCreatePinnedToCore(taskCore0, "TaskCore0", 10000, NULL, 1, NULL, 0);
    xTaskCreatePinnedToCore(taskCore1, "TaskCore1", 10000, NULL, 1, NULL, 1); 
}

void loop() {}

void taskCore0(void *pvParameters) {
    while(1)
    {
#if SERIAL_BT
        // MPU6050的值获取
        if(1)
        {
            Serial.printf("%f,%f,%f\n",Yaw_dmp,Pitch_dmp,Roll_dmp);
            // vTaskDelay(pdMS_TO_TICKS(1));
            // Serial.printf("%d,%d,%d\n",gx_dmp,gy_dmp,gz_dmp);
        }

        // 编码器的值获取
        if(0)
        {
            Serial.printf("Encoder_Left = %d || Encoder_Right = %d\n",Encoder_Left,Encoder_Right);
            vTaskDelay(pdMS_TO_TICKS(1));
        }

        if(0)
        {
            Serial.printf("MOTO_1_PWM = %d || MOTO_2_PWM = %d\n",MOTO1,MOTO2);
            vTaskDelay(pdMS_TO_TICKS(1));
        }
#endif
        vTaskDelay(pdMS_TO_TICKS(1));                 // 留一个1ms的任务延时防止程序卡死，如果有阻塞任务执行后续可以去掉
    }
}

void taskCore1(void *pvParameters) {
    while(1)
    {
        // MPU6050姿态解算
        if(1)
        {
            MPU6050_DMP_GetEuler();
        }

        // 平衡PID计算
        if(PID_Flag && 0)
        {
            Car_PID();
            PID_Flag = 0;
        }
        
        vTaskDelay(pdMS_TO_TICKS(1));                // 留一个1ms的任务延时防止程序卡死，如果有阻塞任务执行后续可以去掉
    }
}